package mines.eerock.sphero.bowling.robot;

import mines.eerock.sphero.bowling.audiobutton.AudioJackButton;
import android.app.Activity;
import android.os.Handler;
import orbotix.robot.base.FrontLEDOutputCommand;
import orbotix.robot.base.RGBLEDOutputCommand;
import orbotix.robot.base.Robot;
import orbotix.robot.base.RobotProvider;
import orbotix.robot.base.RollCommand;

public class AssistiveRobot extends Robot {

	// Make true to use hardware button
	private final static boolean USE_HARDWARE_BUTTON = true;

	// Power Meter Variables
	private final static int MAX_COLOR = 250;
	private final static int COLOR_INCRAMENT = 10;
	private final static int POWER_METER_SPEED = 25; // in miliseconds
	private int mPowerMeterGreen = MAX_COLOR;
	private int mPowerMeterRed = 0;
	private boolean mPowerMeterForward = true;
	protected boolean mPowerMeterRunning = false;	
	
	// Test Settings
	private float mSpeedSetting = 1.0f;
	private long mDriveTimeSetting = 300;
	private int mHeadingSetting;
	private float mStartUpSpeed = 0f;
//	private long mDriveStartTime;
	
	// Direction Meter Variables
	private final static int MAX_DEGREE_LEFT = 20;
	private final static int MAX_DEGREE_RIGHT = 320;
	private final static int DEGREE_INCRAMENT = 2;
	private final static int DIRECTION_METER_SPEED = 50;
	private float mDirectionMeterDegree = 0;
	private boolean mDirectionMeterLeftSide = true;
	private boolean mDirectionMeterForward = true;
	protected boolean mDirectionMeterRunning = false;
	
	// Robot Object
	private Robot mRobot;
	// Robot Characteristics
//	private static final double[] INCHES_PER_MILLISEC = {0,0,0,.0075833,.01766667,.0025,.04325,.04458333,.053,.0596667};
//	private final static int POWER_SETTING = 7;
//	private final static int BUMPER_THRESHOLD = 3;
//	private final static float BUMPER_SPEED = 0.5f;
//	private double mAlleyWidth = 2.6667;
//	private double mAlleyLength = 7.0;	
	// Robot States
	protected int mRobotState = 0;
	private final static int STOPPED = 0;
//	private final static int DRIVING = 1;
	protected boolean mIsBowling = false;
	
	// Button Press Handler
	protected AudioJackButton mButton = null;	
	protected Activity mStartActivity = null;
	
	public AssistiveRobot( final String aRobotId ) {
		mRobot = RobotProvider.getDefaultProvider().findRobot(aRobotId);
	}
	
	/**
	 * Loop power meter 
	 * 
	 * Power meter goes from Green - Yellow - Orange - Red
	 * This can be accomplished by three states
	 * Green(0,250,0) to (250,250,0) to (250,0,0) and back
	 */
	protected void loopPowerMeter() {
		if(mRobot != null) {
			
			// Going from Green to Red
			if( mPowerMeterForward ) {
				// Green is at 250
				if( mPowerMeterGreen >= MAX_COLOR ) {
					
					// Increase Red Color
					mPowerMeterRed += COLOR_INCRAMENT;
					
					// Check for Max Red
					if( mPowerMeterRed >= MAX_COLOR ) {
						// Decrease Green Color
						mPowerMeterGreen -= COLOR_INCRAMENT;
					}
				}
				else {
					
					// Decrease Green Color
					mPowerMeterGreen -= COLOR_INCRAMENT;
					
					// Check for No more Green
					if( mPowerMeterGreen <= 0 ) {
						// Switch Directions
						mPowerMeterForward = !mPowerMeterForward;
					}
				}
			}
			// Going from Red to Green
			else {
				// Red is at 250
				if( mPowerMeterRed >= MAX_COLOR ) {
					
					// Increase Green Color
					mPowerMeterGreen += COLOR_INCRAMENT;
					
					// Check for Max Green
					if( mPowerMeterGreen >= MAX_COLOR ) {
						// Decrease Red Color
						mPowerMeterRed -= COLOR_INCRAMENT;
					}
				}
				else {
					
					// Decrease Red Color
					mPowerMeterRed -= COLOR_INCRAMENT;
					
					// Check for No more Green
					if( mPowerMeterRed <= 0 ) {
						// Switch Directions
						mPowerMeterForward = !mPowerMeterForward;
					}
				}
			}
			
			
			// Output Color to Sphero
			RGBLEDOutputCommand.sendCommand(mRobot, mPowerMeterRed, mPowerMeterGreen, 0);
			
			// Run next iteration of color change in some amount of time later
			// Send a delayed message on a handler
			final Handler handler = new Handler();                               
			handler.postDelayed(new Runnable() {

				@Override
				public void run() {
					// loop power meter, or stop if the user has requested
					if( !mButton.isButtonPressed() ) {	
						loopPowerMeter();
					}
					// reset power variables
					else
					{
						mSpeedSetting = (float) (((((double)(mPowerMeterGreen) / (double)MAX_COLOR) * 0.5) + 
								  (((double)(MAX_COLOR-mPowerMeterRed) / (double)MAX_COLOR)) * 0.5));					
						mPowerMeterGreen = MAX_COLOR;
						mDriveTimeSetting = 300 + (long)((1-(mSpeedSetting*mSpeedSetting))*1000);
						mPowerMeterRed = 0;
						// temporary since these are button functions
						FrontLEDOutputCommand.sendCommand(mRobot, 1f);
						mButton.startButtonListener(mStartActivity);
						loopDirectionMeter();
					}
				}
			}, POWER_METER_SPEED);
		}
	}
	
	/**
	 * Loop direction meter 
	 * 
	 * Direction meter goes from -45deg to 45 deg and back
	 */
	protected void loopDirectionMeter() {
		if(mRobot != null) {
			
			// Going from 0 to 45 and back
			if( mDirectionMeterLeftSide ) {
				// Moving Up Degrees
				if( mDirectionMeterForward ) {
					mDirectionMeterDegree += DEGREE_INCRAMENT;
					// Reached farthest point
					if( mDirectionMeterDegree >= MAX_DEGREE_LEFT ) {
						mDirectionMeterForward = !mDirectionMeterForward;
					}
				}
				// Moving Down Degress
				else {
					mDirectionMeterDegree -= DEGREE_INCRAMENT;
					// Reached mid point with to right side sliding
					if( mDirectionMeterDegree <= 0 ) {
						mDirectionMeterLeftSide = !mDirectionMeterLeftSide;
						mDirectionMeterDegree = 360;
					}
				}
				
			}
			// Goind from 360 to 270 and back
			else {
				// Move Up Degrees
				if( !mDirectionMeterForward ) {
					mDirectionMeterDegree -= DEGREE_INCRAMENT;
 					if( mDirectionMeterDegree <= MAX_DEGREE_RIGHT ) {
 						mDirectionMeterForward = !mDirectionMeterForward;
					}
				}
				// Move Down Degrees
				else {
					mDirectionMeterDegree += DEGREE_INCRAMENT;
					// Reached Mid Point switch to 0 degrees
					if( mDirectionMeterDegree >= 360 ) {
						mDirectionMeterLeftSide = !mDirectionMeterLeftSide;
						mDirectionMeterDegree = 0;
					}
				}
			}
			
			
			// Change heading by 5 degrees
			RollCommand.sendCommand(mRobot, mDirectionMeterDegree, 0f);  
			
			// Run next iteration of color change in some amount of time later
			// Send a delayed message on a handler
			final Handler handler = new Handler();                               
			handler.postDelayed(new Runnable() {

				@Override
				public void run() {
					// loop power meter, or stop if the user has requested
					if( !mButton.isButtonPressed() ) {	
						loopDirectionMeter();
					}
					// reset power variables
					else
					{
						//mDirectionMeterRunning = false;
						mHeadingSetting = (int)mDirectionMeterDegree;	
						mStartUpSpeed = 0;
						mButton.startButtonListener(mStartActivity);
						driveStartUp();
						// Turn off FrontLEDOutput
				        FrontLEDOutputCommand.sendCommand(mRobot, 0f);
						mDirectionMeterForward = true;
						mDirectionMeterDegree = 0;
						mDirectionMeterLeftSide = true;
					}
				}
			}, DIRECTION_METER_SPEED);
		}
	}
	
	/**
	 * Drive Sphero at a UI-determined speed and direction
	 */
	private void drive() {
		if(mRobot != null) {
			
			// Calculate distance traveled
			//double DistanceX = getDistanceTraveledX(System.currentTimeMillis()-mDriveStartTime);
			RGBLEDOutputCommand.sendCommand(mRobot, 255, 0, 0);
			// Compare horizontal distance with the width of the alley
			// Bank of Bumper if it gets too close
			/*if ( false ) {//(DistanceX-BUMPER_THRESHOLD) > (mAlleyWidth / 2.0) ) {
				// Calculate distance traveled
				mHeadingSetting = 360 - mHeadingSetting;
				bump();
			}
			else {*/
				// first parameter is heading (0-360), second is speed (0-1)
				// Send a roll command to Sphero so it goes forward at full speed.
				RollCommand.sendCommand(mRobot, mHeadingSetting, mSpeedSetting);      
	
				// Send a delayed message on a handler
				final Handler handler = new Handler();                               
				handler.postDelayed(new Runnable() {
	
					@Override
					public void run() {
						// Send a stop to Sphero
						RollCommand.sendStop(mRobot); 
						// Output No Color to Sphero
						RGBLEDOutputCommand.sendCommand(mRobot, 0, 0, 255);
						mRobotState = STOPPED;
						if( mIsBowling ) {
							mButton.startButtonListener(mStartActivity);
							startWait();
						}
					}
				}, mDriveTimeSetting);
			//}

		}
	}
	
	/**
	 * Drive Sphero in a circle of radius
	 *
	private void bump() {
		if(mRobot != null) {		
			
			// Drive with Reverse Directions
			RollCommand.sendCommand(mRobot, mHeadingSetting, BUMPER_SPEED);  
			RGBLEDOutputCommand.sendCommand(mRobot, 0, 255, 0);

			// Send a delayed message on a handler
			final Handler handler = new Handler();                               
			handler.postDelayed(new Runnable() {

				@Override
				public void run() {
					// Send a stop to Sphero
					RollCommand.sendStop(mRobot); 
					// Output No Color to Sphero
					RGBLEDOutputCommand.sendCommand(mRobot, 255, 0, 0);
					mDriveStartTime = System.currentTimeMillis();
					drive();
				}
			}, 200);

		}
	}*/
	
	/**
	 * Use a Ramping Power to start driving without any header variance
	 */
	public void driveStartUp() {
		if(mRobot != null) {
			// first parameter is heading (0-360), second is speed (0-1)
			// Send a roll command to Sphero so it goes forward at full speed.
			RollCommand.sendCommand(mRobot, mHeadingSetting, mStartUpSpeed);    
			RGBLEDOutputCommand.sendCommand(mRobot, 0, 255, 0);

			mStartUpSpeed += 0.1f;
			
			// Send a delayed message on a handler
			final Handler handler = new Handler();                               
			handler.postDelayed(new Runnable() {

				@Override
				public void run() {
					// Send a stop to Sphero
					if(mStartUpSpeed < mSpeedSetting) {						
						driveStartUp(); 
					}
					else {
						//mDriveStartTime = System.currentTimeMillis();
						drive();
					}
				}
			}, 100);
		}
	}
	
	/**
	 * Given system time passed in drive, calculate X Distance Traveled
	 * @return Distance Traveled in the X Direction
	 *
	private double getDistanceTraveledX(long Time) {
		
		double Distance = INCHES_PER_MILLISEC[POWER_SETTING-1] * Time;
		double Heading = ((double)mHeadingSetting / 180.0) * Math.PI;
		
		return Distance*Math.sin(Heading);
	}*/
	
	/**
	 * Given system time passed in drive, calculate Y Distance Traveled
	 * @return Distance Traveled in the Y Direction
	 *
	private double getDistanceTraveledY(long Time) {
		double Distance = INCHES_PER_MILLISEC[POWER_SETTING-1] * Time;
		double Heading = ((double)mHeadingSetting / 180.0) * Math.PI;
		
		return Distance*Math.cos(Heading);
	}*/
	
	/**
	 * Start bowling within the activity act
	 * @param act
	 */
	public void startBowling(Activity act) {
		if( USE_HARDWARE_BUTTON ) {
			mStartActivity = act;
			mButton = new AudioJackButton();
			mButton.setupButton(act);
			mButton.startButtonListener(act);
			mIsBowling = true;
			startWait(); 
		} 
	}
	
	/**
	 * Wait for the user to click the physical button, and then go to the power meter
	 */
	private void startWait() {
		// Run next iteration of color change in some amount of time later
		// Send a delayed message on a handler
		final Handler handler = new Handler();                               
		handler.postDelayed(new Runnable() {

			@Override
			public void run() {
				// loop the start waiter
				if( !mButton.isButtonPressed() ) {	
					startWait();
				}
				// reset power variables
				else
				{
					mButton.startButtonListener(mStartActivity);
					loopPowerMeter(); 
				}
			}
		}, 50);
	}
	
    /**
     * cleanUp()
     *
     * Called on App termination or stall, releases audio resources
     */
    public void cleanUp()
    {
    	if( mButton != null ) {
    		mButton.cleanUp();
    	}
    	if( mRobot != null ) {
    		mRobot = null;
    		//Disconnect Robot
            RobotProvider.getDefaultProvider().removeAllControls();
    	}
    }

}

